function [ r_f, eta_n, p_i, target, p_f] = transf_points_for_robot(v, r, p_i, target, p_f, eta_n)
global workspace_data timestep c_vel_pos m_point_window speed_init

% find current p_i
if (p_i < size(workspace_data.traj_m,1))
    p_nearest = find_point(p_i,m_point_window, r);
    if (p_nearest == p_i)
        if ( p_i + (fix(timestep*speed_init/workspace_data.px_to_m_x) + 1) <= size(workspace_data.traj_m,1) )
            p_i = p_i + (fix(timestep*speed_init/workspace_data.px_to_m_x) + 1);
        else
            p_i = size(workspace_data.traj_m,1);
        end
    else
        if (p_nearest <= size(workspace_data.traj_m,1) )
            p_i = p_nearest;
        else
            p_i = size(workspace_data.traj_m,1);
        end    
    end
end

% % distance from current rob pos to p_i
% d = (p_i.^2 + r.^2).^.5;

% find target position (index)
if ( p_i + (fix(timestep * speed_init/workspace_data.px_to_m_x * c_vel_pos) + 1) < size(workspace_data.traj_m,1) )
    target = p_i + (fix(timestep * speed_init/workspace_data.px_to_m_x * c_vel_pos) + 1);
else
    target = size(workspace_data.traj_m,1);
end
% future position
r_f = r + timestep*c_vel_pos*v;
% r_f = r + 1*c_vel_pos*v;

% determine p_i relative to future position
if ( p_f < size(workspace_data.traj_m,1) )
    
    [ p_nearest, eta_n ] = find_point(p_f,m_point_window,r_f);
    
    if ( p_nearest == p_f )
        if ( p_f + (fix(timestep*speed_init/workspace_data.px_to_m_x) + 1) <= size(workspace_data.traj_m,1) )
            p_f = p_f + (fix(timestep*speed_init/workspace_data.px_to_m_x) + 1);
        else
            p_f = size(workspace_data.traj_m,1);
        end
    else
        if (p_nearest <= size(workspace_data.traj_m,1) )
            p_f = p_nearest;
        else
            p_f = size(workspace_data.traj_m,1);
        end
    end
    if norm(eta_n) ~= 0
        eta_n = eta_n/norm(eta_n);
    end
end
end


function [ p_i, eta_n ] = find_point(p_i,N,r)
% searches in neighbourhood N of current rob position r for via point (index p_i) with
% smallest distance to rob position

global workspace_data;

if (p_i - N/8) < 1
    L(1) = 1;
else
    L(1) = p_i - N/8;
end

if (p_i + N*7/8) > size(workspace_data.traj_m,1)
    
    L(2) = size(workspace_data.traj_m,1);
    
else
    
    L(2) = p_i + N*7/8 - 1;
    
end

tmp = zeros(3,(L(2) - L(1)));

for i = L(1):(L(2) - 1)
    eta_t = workspace_data.traj_m((i + 1),:) - workspace_data.traj_m(i,:);
    eta_n = [ eta_t(2) eta_t(1) ];
    
    d = workspace_data.traj_m(i,:) - r;
    
    tmp(1:2,i - L(1) + 1) = eta_n';
    tmp(3,i - L(1) + 1)   = norm(cross([ eta_n 0 ],[ d 0 ]));
end

[ dummy, min_ind ] = min(tmp(3,:));

p_i   = min_ind + L(1) - 1;
eta_n = tmp(1:2,min_ind)';

end